--cc.work_global()
--SEARCH_STATE_NOT_INITIALISED = 0
--SEARCH_STATE_SEARCHING = 1
--SEARCH_STATE_SUCCEEDED = 2
--SEARCH_STATE_FAILED = 3
--SEARCH_STATE_OUT_OF_MEMORY = 4
--SEARCH_STATE_INVALID = 5
--cc.disable_global()
-- 对array[first..last]进行堆筛选
local function sift_heap(array, first, last)
    local i = first             -- 被筛选结点索引
    local j = 2 * i             -- 被筛选结点的左孩子索引
    local temp = array[i]       -- 保存被筛选结点
    
    while (j <= last) do
        if (j < last and array[j].f > array[j + 1].f) then
            j = j + 1           -- 若右孩子较小，把j指向右孩子
        end
        
        if (temp.f > array[j].f) then
            array[i] = array[j] -- 将array[j]调整到双亲结点位置上
            i = j               -- 修改i和j值，指向下一个被筛选结点和被筛选结点的左孩子
            j = 2 * i
        else
            break               -- 已是小根堆，筛选结束
        end
    end
    
    array[i] = temp             -- 被筛选结点的值放入最终位置
end

-- 建立初始小根堆
local function make_heap(array, first, last)
    local n = last - first + 1
    for i = math.floor(n/2), 1, -1 do
        sift_heap(array, i, n)
    end
end

-- 往小根堆中插入一个结点
local function push_heap(array, first, last)
    make_heap(array, first, last)
end

-- 从小根堆中弹出一个结点
local function pop_heap(array, first, last)
    array[first], array[last] = array[last], array[first]  
    make_heap(array, first, last-1)
end

-- 打印数组
local function print_heap(array)
    str = ""
    for i = 1, #array do
        str = str .. array[i] .. "," 
    end
    print("#######################", str)
end


local SeNode = class("SeNode")

function SeNode:ctor()
    self.parent = nil   -- used during the search to record the parent of successor nodes
    self.child = nil    -- used after the search for the application to view the search in reverse
    self.g = 0.0        -- cost of this node + it's predecessors
    self.h = 0.0        -- heuristic estimate of distance to goal
    self.f = 0.0        -- sum of cumulative cost of predecessors and self and heuristic
    self.stNode = nil
end

local AStarSearch = class("AStarSearch")
function AStarSearch:ctor()
    self.allocatedNodeNum = 0
    self.state = SEARCH_STATE_NOT_INITIALISED
    self.curSENode = nil
    self.cancelSearch = false

    self.openList = {}
    self.closedList = {}
    -- Successors is a vector filled out by the user each type successors to a node
    -- are generated
    self.successors = {}

    self.steps = 0

    self.start = nil
    self.goal = nil
end

function AStarSearch:cancelSearch()
    self.cancelSearch = true
end

function AStarSearch:startSearch(_start,_goal)
    self.cancelSearch = false

    self.start = SeNode.new()
    self.goal = SeNode.new()
    self.start.stNode = _start
    self.goal.stNode = _goal

    self.state = SEARCH_STATE_SEARCHING

    self.start.g = 0
    self.start.h = self.start.stNode:goalDistanceEstimate(self.goal.stNode)
    self.start.f = self.start.g+self.start.h
    self.start.parent = nil

    table.insert(self.openList,self.start)
    
    -- Sort back element into heap
    push_heap(self.openList, 1, #self.openList)

    self.steps = 0
end

function AStarSearch:searchStep()
    if self.state ~= SEARCH_STATE_SEARCHING then
        return self.state
    end

    if #self.openList == 0 or self.cancelSearch then
        self:freeAllNodes()
        self.state = SEARCH_STATE_FAILED
        return self.state
    end

    self.steps = self.steps+1

    -- Pop the best node (the one with the lowest f) 
    local node = self.openList[1]    -- get pointer to the node
    pop_heap(self.openList, 1, #self.openList)
    table.remove(self.openList)--remove last

    -- Check for the goal
    if node.stNode:isGoal(self.goal.stNode) then
         -- The user is going to use the Goal Node he passed in 
        -- so copy the parent pointer of n 
        self.goal.parent = node.parent

        -- A special case is that the goal was passed in as the start state
        -- so handle that here
        if not node.stNode:isSameState(self.start.stNode) then
            node = nil

            local nodeChild = self.goal
            local nodeParent = self.goal.parent
            repeat
                nodeParent.child = nodeChild
                nodeChild = nodeParent
                nodeParent = nodeParent.parent
            until nodeChild==self.start;
        end

        -- delete nodes that aren't needed for the solution
        self:freeUnusedNodes()
        self.state = SEARCH_STATE_SUCCEEDED
        return self.state
    else
        self.successors = {}
        local ret = node.stNode:getSuccessors(self,(node.parent and node.parent.stNode or nil))

        if not ret then
            self.successors = {}
            self:freeAllNodes()
            self.state = SEARCH_STATE_OUT_OF_MEMORY
            return self.state
        end

        local successor = nil

        for i=1,#self.successors do
            successor = self.successors[i]

            local new_g = node.g + node.stNode:getCost(successor.stNode)

            local openlist_result = nil
            local openlist_index = 1
            for j=1,#self.openList do
                if self.openList[j].stNode:isSameState(successor.stNode) then
                    openlist_result = self.openList[j]
                    openlist_index = j
                    break
                end
            end

            if openlist_result and openlist_result.g<=new_g then
                successor = nil
            else
                local closedlist_result = nil
                local closedlist_index = 1
                for j=1,#self.closedList do
                    if self.closedList[j].stNode:isSameState(successor.stNode) then
                        closedlist_result = self.closedList[j]
                        closedlist_index = j
                        break;
                    end
                end

                if closedlist_result and closedlist_result.g <= new_g then
                    successor = nil
                else
                    successor.parent = node
                    successor.g = new_g
                    successor.h = successor.stNode:goalDistanceEstimate(self.goal.stNode)
                    successor.f = successor.g + successor.h

                    if closedlist_result then
                        table.remove(self.closedList,closedlist_index)
                        closedlist_result = nil
                    end

                    if openlist_result then
                        table.remove(self.openList,openlist_index)
                        openlist_result = nil

                        make_heap(self.openList,1,#self.openList)
                    end

                    table.insert(self.openList,successor)
                    push_heap(self.openList,1,#self.openList)
                end
            end
        end

        table.insert(self.closedList,node)
    end
    return self.state
end

function AStarSearch:addSuccessor(state)
    local node = SeNode.new()
    if node then
        node.stNode = state
        table.insert(self.successors,node)
        return true
    end
    return false
end

function AStarSearch:freeSolutionNodes()
    local node = self.start
    if self.start.child then
        repeat
            local del = node
            node = node.child
            del = nil
        until node==self.goal;
    else
        self.start = nil
        self.goal = nil
    end
end

function AStarSearch:getSolutionStart()
    self.curSENode = self.start
    if self.start then
        return self.start.stNode
    else
        return nil
    end
end
    
-- Get next node
function AStarSearch:getSolutionNext()
    if (self.curSENode) then
        if (self.curSENode.child) then
            local child = self.curSENode.child

            self.curSENode = self.curSENode.child

            return child.stNode
        end
    end

    return nil
end

-- Get end node
function AStarSearch:getSolutionEnd()
    self.curSENode = self.goal
    if (self.goal) then
        return self.goal.stNode
    else
        return nil
    end
end
    
-- Step solution iterator backwards
function AStarSearch:getSolutionPrev()
    if (self.curSENode) then
        if (self.curSENode.parent) then
            local parent = self.curSENode.parent
            self.curSENode = self.curSENode.parent
            return parent.stNode
        end
    end

    return nil
end

-- This is called when a search fails or is cancelled to free all used
-- memory 
function AStarSearch:freeAllNodes()
    self.m_OpenList = {}
    self.m_ClosedList = {}
end

-- This call is made by the search class when the search ends. A lot of nodes may be
-- created that are still present when the search ends. They will be deleted by this 
-- routine once the search ends
function AStarSearch:freeUnusedNodes()
    self.m_OpenList = {}
    self.m_ClosedList = {}
end

return AStarSearch